/*=+--+=#=+--     Unmanned Aerial System Management Software      --+=#=+--+=#*\
|          Copyright (C) 2011 Regents of the University of Colorado.           |
|                             All Rights Reserved.                             |

     This program is free software: you can redistribute it and/or modify
     it under the terms of the GNU General Public License version 2 as
     published by the Free Software Foundation.

     This program is distributed in the hope that it will be useful,
     but WITHOUT ANY WARRANTY; without even the implied warranty of
     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
     GNU General Public License for more details.

     You should have received a copy of the GNU General Public License
     along with this program.  If not, see <http://www.gnu.org/licenses/>.

            Jack Elston                       Cory Dixon                        
|           elstonj@colorado.edu              dixonc@colorado.edu              |
|                                                                              |
\*=+--+=#=+--                 --+=#=+--+=#=+--                    --+=#=+--+=#*/

#include <iostream.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h> // -dw- for memcpy

#include <sys/param.h>
#include <unistd.h>
#include <fcntl.h>
#include <sys/ioctl.h>
#include <linux/joystick.h>
#include <errno.h>

#define _JS_MAX_AXES 9

void close ();
void rawRead ( int *buttons, float *axes );
void read ( int *buttons, float *axes );
float fudge_axis ( float value, int axis );
void open ();
void print() ;
void printHead() ;

/* check the joystick driver version */

js_corr      jsCorr;
js_event     js;
int          tmp_buttons ;
float        tmp_axes [ _JS_MAX_AXES ] ;
int          error    ;
char         name [ 128 ] ;
char         fname [ 128 ] ;
int          fd       ;

int          num_axes ;
int          num_buttons ;
float dead_band [ _JS_MAX_AXES ] ;
float saturate  [ _JS_MAX_AXES ] ;
float center    [ _JS_MAX_AXES ] ;
float max       [ _JS_MAX_AXES ] ;
float min       [ _JS_MAX_AXES ] ;

float axis[_JS_MAX_AXES];    // point of axes values
int   buttons;                // the actual buttons

int main()
{

	cout << "Joystick Driver Version: 0x" << hex << JS_VERSION << endl;

	sprintf ( fname, "/dev/input/js%d", 0 ) ;
	if ( access ( fname, F_OK ) != 0 )
		sprintf ( fname, "/dev/js%d", 0 ) ;

	open();

	cout << "Joystick Name: " << name << endl
		<< "Number of Axis: " << num_axes << endl
		<< "Number of Buttons: " << num_buttons << endl;

	cout << "Joystick Corrections: prec=" << jsCorr.prec << " type=" << jsCorr.type << endl;
	for( int i=0; i<8; i++ )
		cout << " coef[" << i << "] = " << jsCorr.coef[i];
	cout << endl;

	printHead();
	while(1) {
		read(&buttons, axis);
		print();
		printf("\r");
	}

	close();

	return 0;
}

void open ()
{
	for ( int i = 0 ; i < _JS_MAX_AXES ; i++ )
		tmp_axes [ i ] = 0.0f ;

	tmp_buttons = 0 ;
	fd = ::open ( fname, O_RDONLY ) ;

	error = ( fd < 0 ) ;

	if ( error )
		return ;

	//ioctl ( fd, JSIOCGCORR, jsCorr);
	ioctl ( fd, JS_GET_CAL, jsCorr);
	ioctl ( fd, JSIOCGAXES   , & num_axes    ) ;
	ioctl ( fd, JSIOCGBUTTONS, & num_buttons ) ;
	ioctl ( fd, JSIOCGNAME ( sizeof(name) ), name ) ;
	fcntl ( fd, F_SETFL, O_NONBLOCK ) ;

	if ( num_axes > _JS_MAX_AXES )
		num_axes = _JS_MAX_AXES ;
	for ( int i = 0 ; i < _JS_MAX_AXES ; i++ )
	{
		max [ i ] = 32767.0f ;
		center [ i ] = 0.0f ;
		min [ i ] = -32767.0f ;
		dead_band [ i ] = 0.0f ;
		saturate  [ i ] = 1.0f ;
	}

}

void close ()
{
	if ( ! error )
		::close ( fd ) ;
}

float fudge_axis ( float value, int axis ) 
{
	if ( value < center[axis] )
	{
		float xx = (      value    - center[ axis ] ) /
			( center [ axis ] - min [ axis ] ) ;

		if ( xx < -saturate [ axis ] )
			return -1.0f ;

		if ( xx > -dead_band [ axis ] )
			return 0.0f ;

		xx = (        xx         + dead_band [ axis ] ) /
			( saturate [ axis ] - dead_band [ axis ] ) ;

		return ( xx < -1.0f ) ? -1.0f : xx ;
	} else {
		float xx = (     value    - center [ axis ] ) /
			( max [ axis ] - center [ axis ] ) ;

		if ( xx > saturate [ axis ] )
			return 1.0f ;

		if ( xx < dead_band [ axis ] )
			return 0.0f ;

		xx = (        xx         - dead_band [ axis ] ) /
			( saturate [ axis ] - dead_band [ axis ] ) ;

		return ( xx > 1.0f ) ? 1.0f : xx ;
	}
}

void read ( int *buttons, float *axes )
{
	if ( error )
	{
		if ( buttons )
			*buttons = 0 ;

		if ( axes )
			for ( int i = 0 ; i < num_axes ; i++ )
				axes[i] = 0.0f ;
	}

	float raw_axes [ _JS_MAX_AXES ] ;

	rawRead ( buttons, raw_axes ) ;

	if ( axes )
		for ( int i = 0 ; i < num_axes ; i++ )
			axes[i] = fudge_axis ( raw_axes[i], i ) ;
}

void rawRead ( int *buttons, float *axes )
{
	if ( error )
	{
		if ( buttons )
			*buttons = 0 ;

		if ( axes )
			for ( int i = 0 ; i < num_axes ; i++ )
				axes[i] = 1500.0f ;

		return ;
	}
	while (1)
	{
		int status = ::read ( fd, &js, sizeof(js_event) ) ;

		if ( status != sizeof(js_event) )
		{
			/* use the old values */
			cout << "Using the old interface" << endl;

			if ( buttons ) *buttons = tmp_buttons ;
			if ( axes    ) memcpy ( axes, tmp_axes, sizeof(float) * num_axes ) ;

			if ( errno == EAGAIN )
				return ;

			perror( fname ) ;
			error = 1 ;
			return ;
		}

		switch ( js.type & ~JS_EVENT_INIT )
		{
			case JS_EVENT_BUTTON :
				if ( js.value == 0 ) /* clear the flag */
					tmp_buttons &= ~(1 << js.number) ;
				else
					tmp_buttons |=  (1 << js.number) ;
			break ;

		case JS_EVENT_AXIS:
			if ( js.number < num_axes )
			{
				tmp_axes [ js.number ] = (float) js.value ;

				if ( axes )
				memcpy ( axes, tmp_axes, sizeof(float) * num_axes ) ;
			}
			break ;

		default:
			perror( "PLIB_JS: Unrecognized /dev/js return!?!" ) ;

			/* use the old values */

			if ( buttons ) *buttons = tmp_buttons ;
			if ( axes    ) memcpy ( axes, tmp_axes, sizeof(float) * num_axes ) ;
			return ;
		}

		if ( buttons )
			*buttons = tmp_buttons ;
	}

} 

        void printHead() {
                printf ("+-------------------------------------------------------+\n") ;
                printf ("|    Buttons      |            Axes                     |\n" ) ;
                printf ("| ");
                for ( int j = 0 ; j < num_buttons; j++ )
                        printf ( "%1d ", j%8 ) ;
                printf ("| ");
                for ( int j = 0 ; j < num_axes; j++ )
                        printf ( "  %1d   ", j ) ;
                printf ("|\n");
                printf ("+-------------------------------------------------------+\n") ;
        }

        // print the body of the joystick, which
        // can be overwritten to watch values change
        void print() {
                printf( "| " );
                for(int i=0; i < num_buttons; i++)
                        printf ( "%01x ", ((buttons >> i) & 0x1) ) ;
                printf ("| ");
                for(int i=0; i < num_axes; i++)
                        printf ( "%+1.2f ", axis[i] ) ;
                printf ( "| " ) ;
        }

